/*----------------------------------------------------------------------------*/
/* Copyright 2011 http://www.kmftech.com                                      */
/*                                                                            */
/* This file is part of kmftech.                                              */
/*                                                                            */
/* kmftech is free software: you can redistribute it and/or modify it under   */
/* the terms of the GNU General Public License as published by the Free       */
/* Software Foundation, version 2 of the License, or (at your option) any     */
/* later version.                                                             */
/*                                                                            */
/* kmftech is distributed in the hope that it will be useful, but WITHOUT ANY */
/* WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS  */
/* FOR A PARTICULAR PURPOSE. See the GNU General Public License for more      */
/* details.                                                                   */
/*                                                                            */
/* You should have received a copy of the GNU General Public License along    */
/* with kmftech. If not, see http://www.gnu.org/licenses/.                    */
/*----------------------------------------------------------------------------*/


/*----------------------------------------------------------------------------*/
/*---------------------------- DOCUMENT HISTORY ------------------------------*/
/* Ver: 1.0    Author: Karim Mansour                      Date: 07 June  2011 */
/* Ver: 1.1    Author: Karim Mansour                      Date: 08 June  2011 */
/* Ver: X.X    Author:                                    Date:               */
/*----------------------------------------------------------------------------*/
/*---------------------    DOCUMENT CHANGE HISTORY ---------------------------*/
/* Ver: 1.0    Change: Initial Version.                                       */
/* Ver: 1.1    Change: Fixed bug of division,and used bsc.h instead bsc_pwm   */
/* Ver: X.X    Change:                                                        */
/*----------------------------------------------------------------------------*/


/*                           Needed Header Files                              */
#include "kmftech_types.h"
#include "bsc.h"


/*                      Servo Driver Global Declarations                      */
#define NB_SERVO_MOTORS (2)
#define SERVO_PULSE_PERIOD_US (20000)
#define SERVO_PULSE_MIN_DURATION_US (600)
#define SERVO_PULSE_MAX_DURATION_US (2400)

/*                           Functions Prototypes                             */
void vd_drv_servo_set_angle(uint8 u8_servo_id, uint8 u8_servo_angle);

/*                         Functions Implementation                           */

/*----------------------------------------------------------------------------*/
/*---------------------------- FUNCTION HEADER -------------------------------*/
/* Name       : vd_drv_servo_set_angle                                        */
/* Description: Set servo angle                                               */
/* Input(s)   : Input  1: uint8 - Servo ID                                    */
/* Input(s)   : Input  2: uint8 - Servo angle                                 */
/* Output(s)  : Output 1: void                                                */
/* Author     : Karim Mansour                                                 */
/*----------------------------------------------------------------------------*/
void vd_drv_servo_set_angle(uint8 u8_servo_id, uint8 u8_servo_angle)
{
   uint16 u16Local_duty_us;
   uint32 u32Local_temp;
   u32Local_temp = (uint32)((uint32)SERVO_PULSE_MAX_DURATION_US - (uint32)SERVO_PULSE_MIN_DURATION_US);
   u32Local_temp = u32Local_temp * (uint32)u8_servo_angle;
   u32Local_temp = u32Local_temp / 180;
   u32Local_temp = u32Local_temp + (uint32)SERVO_PULSE_MIN_DURATION_US;
   u16Local_duty_us = (uint16) u32Local_temp;

   vd_bsc_pwm_set(u8_servo_id, u16Local_duty_us, (uint16)SERVO_PULSE_PERIOD_US);
}
/*-----------------------------End Of File------------------------------------*/